#include "UltrasonicSensor_L.h"

uint64_t UltrasonicSensor::myPulseIn(uint32_t pin, uint32_t value, long maxDuration) {
    int pulse = value == HIGH ? 1 : 0;
    uint64_t tick = micros();
    uint64_t maxd = (uint64_t)maxDuration;
    while (digitalRead(pin) != pulse) {
        if (micros() - tick > maxd)
            return 0;
    }
    uint64_t start = micros();
    while (digitalRead(pin) == pulse) {
        if (micros() - tick > maxd)
            return 0;
    }
    uint64_t end = micros();
    return end - start;
}

float UltrasonicSensor::getDistanceCM(int sensorPin) {
    pinMode(sensorPin, OUTPUT);
    digitalWrite(sensorPin, LOW);
    delayMicroseconds(2);

    digitalWrite(sensorPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(sensorPin, LOW);

    pinMode(sensorPin, INPUT);
    long duration = myPulseIn(sensorPin, HIGH, 100000);
    float distance = duration / 58.0;
    delay(10);
    return distance;
}
